/**
*@note Copyright 2012-2022 CETHIK All Rights Reserved.
*@brief 一堆要用到的公式 
*@author tuyinan
*@data 2022-04-xx
*/
#ifndef COSTMAP_2D_COSTMAP_MATH_H_
#define COSTMAP_2D_COSTMAP_MATH_H_

#include <costmap_2d/msg_struct.h>
#include <math.h>

#include <Eigen/Eigen>
#include <algorithm>
#include <iostream>
#include <vector>

using namespace costmap_2d;
/**
*@brief -1 如果x < 0, 否则+1
*@param double x 值
*@return double 结果
*@author: tuyinan
*@version: 2022-04-xx
*/
inline double sign(double x) { return x < 0.0 ? -1.0 : 1.0; }
/**
*@brief -1 如果x < 0, 等于0为0,否则+1
*@param double x 值
*@return double 结果
*@author: tuyinan
*@version: 2022-04-xx
*/
inline double sign0(double x) { return x < 0.0 ? -1.0 : (x > 0.0 ? 1.0 : 0.0); }
/**
*@brief 计算距离
*@param double x0 第一个值的x
*@param double y0 第一个值的y
*@param double x1 第二个值的x
*@param double y1 第二个值的y
*@return double 结果
*@author: tuyinan
*@version: 2022-04-xx
*/
inline double distance(double x0, double y0, double x1, double y1) {
  return hypot(x1 - x0, y1 - y0);
}
/**
*@brief 四元转欧拉角
*@param const costmap_2d::Quaternion& q 四元数值
*@return double 欧拉角度
*@author: tuyinan
*@version: 2022-04-xx
*/
inline double getYaw(const costmap_2d::Quaternion& q) {
  double yaw;

  double sqw;
  double sqx;
  double sqy;
  double sqz;

  sqx = q.x * q.x;
  sqy = q.y * q.y;
  sqz = q.z * q.z;
  sqw = q.w * q.w;

  // Cases derived from https://orbitalstation.wordpress.com/tag/quaternion/
  double sarg =
      -2 * (q.x * q.z - q.w * q.y) /
      (sqx + sqy + sqz + sqw); /* normalization added from urdfom_headers */

  if (sarg <= -0.99999) {
    yaw = -2 * atan2(q.y, q.x);
  } else if (sarg >= 0.99999) {
    yaw = 2 * atan2(q.y, q.x);
  } else {
    yaw = atan2(2 * (q.x * q.y + q.w * q.z), sqw + sqx - sqy - sqz);
  }
  return yaw;
};
/**
*@brief 对点进行坐标转换
*@param costmap_2d::PointCloud* pt 要处理的点
*@param Eigen::Transform<float, 3, Eigen::Isometry> transform 转换矩阵
*@return void
*@author: tuyinan
*@version: 2022-04-xx
*/
void doTransform(costmap_2d::PointCloud* pt,
                 Eigen::Transform<float, 3, Eigen::Isometry> transform);
/**
*@brief 点到线的距离
*@param double pX 点的x
*@param double pY 点的y
*@param double x0 线第一个值的x
*@param double y0 线第一个值的y
*@param double x1 线第二个值的x
*@param double y1 线第二个值的y
*@return double 结果
*@author: tuyinan
*@version: 2022-04-xx
*/
double distanceToLine(double pX, double pY, double x0, double y0, double x1,
                      double y1);

#endif  // COSTMAP_2D_COSTMAP_MATH_H_
